Learning Objectives
# === Odometry Update Function ===
def update_odometry():
global prev_left, prev_right, pose
cur_left = left_sensor.getValue()
cur_right = right_sensor.getValue()
dphi_l = cur_left - prev_left
dphi_r = cur_right - prev_right
prev_left = cur_left
prev_right = cur_right
delta_s = WHEEL_RADIUS * (dphi_r + dphi_l) / 2.0
delta_theta = WHEEL_RADIUS * (dphi_r - dphi_l) / WHEEL_BASE
theta_mid = pose[2] + delta_theta / 2.0
pose[0] += delta_s * math.cos(theta_mid)
pose[1] += delta_s * math.sin(theta_mid)
pose[2] += delta_theta
pose[2] = (pose[2] + math.pi) % (2 * math.pi) - math.pi
return delta_s, delta_theta
the odometry reports
Suppose it should be x = 0; y = 0; θ = 360°
What if we ran more laps?
16 laps :
the odometry reports:
64 laps:
The problem comes from Angular Drift!!
Add a compass sensor to the robot.
Search for "compass"
Getting compass values (Sample Code)
import math
from controller import Robot
compass = robot.getDevice("compass")
compass.enable(TIME_STEP)
def get_heading_angle(compass_values):
rad = math.atan2(compass_values[0], compass_values[1])
if rad < 0:
rad += 2 * math.pi
return rad
def compass_reading():
compass_values = compass.getValues()
current_heading = get_heading_angle(compass_values)
print(f"Compass: {math.degrees(current_heading) % 360:.2f}")
Take note: the Webots simulator's compass has no noise, which never happens in the real world!
the odometry reports:
As for orientation:
Diff = (360° - 347.8° (from wheel encoder) ) + 0.85° (compass) = 13.05° degree
Let's add some noise to the compass sensor to make it more realistic
def get_heading_angle(compass_values):
noise_std_deg = 2.0
rad = math.atan2(compass_values[0], compass_values[1])
if rad < 0:
rad += 2 * math.pi
noise = math.radians(random.gauss(0, noise_std_deg))
noisy_heading = (rad + noise) % (2 * math.pi)
return noisy_heading
It is what we likely to have in the real world
Now, we have two sensors and each of them has their strength and weakness.
Compass sensor (with noise):
Wheel encoder (for orientation):
It is possible for us to combine them?
A complementary filter is a simple and effective sensor fusion technique that combines two (or more) measurements that complement each other in the frequency domain—one sensor gives reliable high-frequency data, and the other gives reliable low-frequency data.
The core idea is to blend the strengths of two sensors:
Mathematically, a complementary filter for combining two signals A(t)A(t)A(t) and B(t)B(t)B(t) can be expressed as:
The complementary filter might look like:
Where:
θencoder: orientation change from encoders
θcompass: absolute heading from compass
“Trust the encoder for how things have changed.”
“Trust the compass for the long-term heading.”
Blend the two to get a more reliable heading.
How do we tune the α?
if α is 0, then we trust the compass 100%, and encoder 0%.
The result of 16 laps.
if α is 1.0, then we trust the compass 0%, and encoder 100%.
The result of 16 laps.
As mentioned earlier, α is a tuning constant (usaully between 0.90 to 0.98)
After some trial and error, we found that the best tuning constant for α is 0.94.
The result of 16 laps.
The result looks promising in this case. However, it is not yet a real-world scenario, as the slippery model of the wheel has not been considered.
# Compute velocities
def update_odometry(dt):
'''
'''
linear_velocity = delta_s / dt
angular_velocity = delta_theta_enc / dt
dt = TIME_STEP / 1000.0 # Convert ms to seconds
_, delta_theta = update_odometry(dt)
def add_wheel_noise(delta, velocity):
# Add a velocity-correlated bias and Gaussian noise
drift = 0.02 * velocity * random.uniform(-1, 1) # dynamic bias
noise = random.gauss(0, 0.05) # random white noise
return delta + drift + noise
let's see the outcome of 16 laps after adding the Wheel Slippery Model with respect to angular velocity.
it clearly shows that the complimentary cannot handle the dynamic noise.
particularly noise that violates the assumptions of complementary filters. e.g., low-frequency bias drift, correlated or state-dependent noise
Complementary filters assume high-frequency noise and no bias. They fail with slow drift or non-Gaussian dynamics like the velocity-correlated drift above.
How to solve this problem? we need a better filtering algorithm! The Kalman filter!